//
// Created by 刘洋 on 2024/2/4.
//
#include "PID.h"
#include "stdlib.h"
float pidAlgorithm(PID* pid,float P,float I,float D,float target,float I_limit,float output_limit,float current_value)
{
    pid->P = P;
    pid->I = I;
    pid->D = D;
    pid->target = target;
    pid->current_error = pid->target - current_value;
    pid->I_error += pid->I * pid->current_error;//I*error的积分
    //积分限幅
    pid->I_error = _constrain(pid->I_error,-I_limit,I_limit);
    pid->output = pid->P*pid->current_error + pid->I_error + pid->D*(pid->current_error-pid->last_error);
    //输出限幅
    pid->output = _constrain(pid->output,-output_limit,output_limit);
    pid->last_error = pid->current_error;
    return pid->output;
}